map_base.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Fwd refs
namespace wolf
{
class Problem;
}  // namespace wolf
 
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/map/factory_map.h"
#include "core/landmark/factory_landmark.h"
#include "core/landmark/landmark_base.h"
 
// other includes
#include "yaml-schema-cpp/yaml_server.hpp"
 
namespace wolf
{
/*
 * Macro for defining Autoconf map creator.
 *
 * Place a call to this macro inside your class declaration (in the map_class.h file),
 * preferably just after the constructors. Inside 'public' keyword.
 *
 * In order to use this macro, the derived map class, MapClass,
 * must have a constructor available with the API:
 *
 *   MapClass(const YAML::Node& _params);
 *
 * Also, there should be the schema file 'MapClass.schema' containing the specifications
 * of the user input yaml file.
 *
 * NOTE: We use `new` and not `make_shared` since the constructor may (should) be private/protected
 */
#define WOLF_MAP_CREATE(MapClass)                                                                                     \
    static MapBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {})     \
    {                                                                                                                 \
        MapBasePtr map;                                                                                               \
        if (not _folders_schema.empty())                                                                              \
        {                                                                                                             \
            auto server = yaml_schema_cpp::YamlServer(_folders_schema);                                               \
            server.setYaml(_input_node);                                                                              \
            if (not server.applySchema(#MapClass))                                                                    \
            {                                                                                                         \
                WOLF_ERROR(server.getLog());                                                                          \
                return nullptr;                                                                                       \
            }                                                                                                         \
            map = std::shared_ptr<MapClass>(new MapClass(server.getNode()));                                          \
        }                                                                                                             \
        else                                                                                                          \
            map = std::shared_ptr<MapClass>(new MapClass(_input_node));                                               \
        if (_input_node["landmarks"])                                                                                 \
        {                                                                                                             \
            for (unsigned int i = 0; i < _input_node["landmarks"].size(); i++)                                        \
            {                                                                                                         \
                LandmarkBasePtr lmk_ptr =                                                                             \
                    FactoryLandmark::create(_input_node["landmarks"][i]["type"].as<std::string>(),                    \
                                            _input_node["landmarks"][i],                                              \
                                            _folders_schema);                                                         \
                lmk_ptr->link(map);                                                                                   \
            }                                                                                                         \
        }                                                                                                             \
        return map;                                                                                                   \
    }                                                                                                                 \
    static MapBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema)      \
    {                                                                                                                 \
        auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);                                   \
        if (not server.applySchema(#MapClass))                                                                        \
        {                                                                                                             \
            WOLF_ERROR(server.getLog());                                                                              \
            return nullptr;                                                                                           \
        }                                                                                                             \
        return create(server.getNode(), {});                                                                          \
    }
 
// class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
    friend LandmarkBase;
 
  private:
    LandmarkBasePtrList landmark_list_;
 
  protected:
    MapBase();
    MapBase(const YAML::Node& _params);
    MapBase(const std::string& _type, const YAML::Node& _params);
 
  public:
    WOLF_MAP_CREATE(MapBase);
 
    ~MapBase() override = default;
    bool hasChildren() const override;
 
  protected:
    virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
    virtual void            removeLandmark(LandmarkBasePtr _landmark_ptr);
 
  public:
    LandmarkBaseConstPtrList getLandmarkList() const;
    LandmarkBasePtrList      getLandmarkList();
    LandmarkBaseConstPtr     getLandmark(const unsigned int& _id) const;
    LandmarkBasePtr          getLandmark(const unsigned int& _id);
 
    virtual void load(std::string _map_file_yaml);
    virtual bool save(std::string _map_file_yaml, const std::string& _map_name = "") const;
 
    virtual void printHeader(int           depth,        //
                             bool          factored_by,  //
                             bool          metric,       //
                             bool          state_blocks,
                             std::ostream& stream,
                             std::string   _tabs = "") const;
    virtual void print(int           depth,        //
                       bool          factored_by,  //
                       bool          metric,       //
                       bool          state_blocks,
                       std::ostream& stream = std::cout,
                       std::string   _tabs  = "") const;
 
    virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
    virtual bool     check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
};
 
inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
{
    LandmarkBaseConstPtrList list_const;
    for (auto&& obj_ptr : landmark_list_) list_const.push_back(obj_ptr);
    return list_const;
}
 
inline LandmarkBasePtrList MapBase::getLandmarkList()
{
    return landmark_list_;
}
 
}  // namespace wolf